#!/usr/bin/env python
# --*-- coding:utf-8 --*--
# author:g-y-b time:2020/6/3

import cv2
import numpy as np


def _claHaris(cov, k):
    '''
    计算角点的响应值R
    :param cov:用来计算M的值
    :param k: k值
    :return:返回R值矩阵
    '''
    result = np.zeros([cov.shape[0], cov.shape[1]], dtype=np.float32)
    for i in range(cov.shape[0]):
        for j in range(cov.shape[1]):
            a = cov[i, j, 0]
            b = cov[i, j, 1]
            c = cov[i, j, 2]
            result[i, j] = a * c - b * b - k * (a + c) * (a + c)
    return result


def myCornerHarris(img, blocksize=2, ksize=3, k=0.04):
    # 计算水平和垂直方向的梯度
    Dx = cv2.Sobel(img, cv2.CV_32F, 1, 0, ksize=ksize)
    Dy = cv2.Sobel(img, cv2.CV_32F, 0, 1, ksize=ksize)

    cov = np.zeros([img.shape[0], img.shape[1], 3], dtype=np.float32)

    for i in range(img.shape[0]):
        for j in range(img.shape[1]):
            cov[i, j, 0] = Dx[i, j] * Dx[i, j]  # 图像水平梯度平方
            cov[i, j, 1] = Dx[i, j] * Dy[i, j]  # 图像水平梯度*图像竖直梯度
            cov[i, j, 2] = Dy[i, j] * Dy[i, j]  # 图像竖直梯度平方

    # 计算块内的梯度和w(x,y)
    # cov = cv2.boxFilter(cov, -1, (blocksize, blocksize), normalize=False)  # 方框滤波
    cov = cv2.GaussianBlur(cov, (blocksize, blocksize), 1)  # 高斯滤波，
    return _claHaris(cov, k)


if __name__ == '__main__':
    img = cv2.imread("./SIFTimg/5.jpg")
    gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    result = myCornerHarris(gray_img, 3, 3, 0.04)
    pos = cv2.goodFeaturesToTrack(result, 5, 0.01, 100)  # cv2.goodFeaturesToTrack()此函数可以获得图像当中最好的N个角点
    for i in range(len(pos)):
        cv2.circle(img, (pos[i][0][0], pos[i][0][1]), 5, [0, 0, 255], thickness=3)
    cv2.imshow('harris1', img)

    img = cv2.imread("./SIFTimg/7.jpg")
    gray_img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    result = myCornerHarris(gray_img, 3, 3, 0.04)
    pos = cv2.goodFeaturesToTrack(result, 5, 0.01, 100)  # cv2.goodFeaturesToTrack()此函数可以获得图像当中最好的N个角点
    for i in range(len(pos)):
        cv2.circle(img, (pos[i][0][0], pos[i][0][1]), 5, [0, 0, 255], thickness=3)
    cv2.imshow('harris2', img)
    cv2.waitKey(0)
